// This is a demo code for using a SSD model to do detection.
// The code is modified from examples/cpp_classification/classification.cpp.
// Usage:
//    ssd_detect [FLAGS] model_file weights_file list_file
//
// where model_file is the .prototxt file defining the network architecture, and
// weights_file is the .caffemodel file containing the network parameters, and
// list_file contains a list of image files with the format as follows:
//    folder/img1.JPEG
//    folder/img2.JPEG
// list_file can also contain a list of video files with the format as follows:
//    folder/video1.mp4
//    folder/video2.mp4
//
#include <caffe/caffe.hpp>
#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <algorithm>
#include <iomanip>
#include <iosfwd>
#include <memory>
#include <string>
#include <utility>
#include <vector>
#include <sstream>

#include <stdlib.h>
#include <stdio.h>
#include <iostream>
using namespace caffe;  // NOLINT(build/namespaces)

/***************RUAS***************************/
class CFilt{

    private:
        int row;
        int col;
        cv::Mat trans_func;
        cv::Mat wiener_filter;

    public:
        CFilt(int irow, int icol);
        void get_wf(float k, float r);
        cv::Mat wiener(cv::Mat img, float k, float r);
        cv::Mat wiener(cv::Mat img);
        cv::Mat clahe(cv::Mat img);
        ~CFilt(){}
};

CFilt::CFilt(int irow, int icol){
    row = irow;
    col = icol;
    trans_func = cv::Mat::zeros(row, col, CV_32FC1);
    wiener_filter = cv::Mat::zeros(row, col, CV_32FC1);
}


void CFilt::get_wf(float k, float r){

    int nRows = row;
    int nCols = col;
    int u = 0;
    int v = 0;

    if(trans_func.isContinuous())
    {
        nCols *= row;
        nRows = 1;
        float* data = trans_func.ptr<float>(0);

        for (int p = 0; p < nCols; ++p){
            v = p / (trans_func.step/4);
            u = p % (trans_func.step/4);
            data[p] = exp(-k * pow(10, -6) * pow(pow(u-col/2, 2) + pow(v-row/2, 2), 5.0/6));
        }
    }
    else {
        for (v = 0; v < nRows; ++v)
        {
            float* data = trans_func.ptr<float>(v);
            for (u = 0; u < nCols; ++u)
            {
                data[u] = exp(-(float)k * pow(10, -6) * pow(pow(u-col/2, 2) + pow(v-row/2, 2), 5.0/6));
            }
        }
    }
    //wiener_filter = ((trans_func.mul(trans_func))/(trans_func.mul(trans_func) + (float)r*0.1)) / trans_func;
    wiener_filter = trans_func.mul(trans_func) / (trans_func.mul(trans_func) + r*0.1) / trans_func;
    //cout<< wiener_filter <<endl;
//    cv::imshow("trans", trans_func);
}

cv::Mat CFilt::wiener(cv::Mat img, float k, float r){

    vector<cv::Mat> BGR;
    cv::Mat BC, BC_dft, BC_ds, BC_inv;
    cv::Mat GC, GC_dft, GC_ds, GC_inv;
    cv::Mat RC, RC_dft, RC_ds, RC_inv;
    cv::Mat img_ds;

    this->get_wf(k, r);
    img.convertTo(img, CV_32F);
    normalize(img, img, 0, 1, CV_MINMAX);
    //normalize(img, img, 0.0, 1.0, NORM_MINMAX);
    split(img, BGR);
    BGR.back().copyTo(RC);
    BGR.pop_back();
    BGR.back().copyTo(GC);
    BGR.pop_back();
    BGR.back().copyTo(BC);
    BGR.pop_back();

    cv::dft(BC, BC_dft, cv::DFT_REAL_OUTPUT);
    BC_ds = BC_dft.mul(wiener_filter);
    cv::dft(BC_ds, BC_inv, cv::DFT_INVERSE|cv::DFT_REAL_OUTPUT);
    cv::normalize(BC_inv, BC_inv, 0, 1, CV_MINMAX);

    cv::dft(GC, GC_dft, cv::DFT_REAL_OUTPUT);
    GC_ds = GC_dft.mul(wiener_filter);
    cv::dft(GC_ds, GC_inv, cv::DFT_INVERSE|cv::DFT_REAL_OUTPUT);
    cv::normalize(GC_inv, GC_inv, 0, 1, CV_MINMAX);

    cv::dft(RC, RC_dft, cv::DFT_REAL_OUTPUT);
    RC_ds = RC_dft.mul(wiener_filter);
    cv::dft(RC_ds, RC_inv, cv::DFT_INVERSE|cv::DFT_REAL_OUTPUT);
    cv::normalize(RC_inv, RC_inv, 0, 1, CV_MINMAX);

    BGR.push_back(BC_inv);
    BGR.push_back(GC_inv);
    BGR.push_back(RC_inv);

    cv::merge(BGR, img_ds);
    cv::normalize(img_ds, img_ds, 0, 255, cv::NORM_MINMAX);
    img_ds.convertTo(img_ds, CV_8U);

    return img_ds;
}

cv::Mat CFilt::wiener(cv::Mat img){

    vector<cv::Mat> BGR;
    cv::Mat BC, BC_dft, BC_ds, BC_inv;
    cv::Mat GC, GC_dft, GC_ds, GC_inv;
    cv::Mat RC, RC_dft, RC_ds, RC_inv;
    cv::Mat img_ds;

    cv::Ptr<cv::CLAHE> clahe = cv::createCLAHE();
    clahe->setClipLimit(3);

    img.convertTo(img, CV_32F);
    cv::normalize(img, img, 0, 1, CV_MINMAX);
    //normalize(img, img, 0.0, 1.0, NORM_MINMAX);
    cv::split(img, BGR);
    BGR.back().copyTo(RC);
    BGR.pop_back();
    BGR.back().copyTo(GC);
    BGR.pop_back();
    BGR.back().copyTo(BC);
    BGR.pop_back();

    cv::dft(BC, BC_dft, cv::DFT_REAL_OUTPUT);
    BC_ds = BC_dft.mul(wiener_filter);
    cv::dft(BC_ds, BC_inv, cv::DFT_INVERSE|cv::DFT_REAL_OUTPUT);
    cv::normalize(BC_inv, BC_inv, 0, 1, cv::NORM_MINMAX);
    //normalize(BC_inv, BC_inv, 0, 255, NORM_MINMAX);
    //BC_ds.convertTo(BC_inv, CV_8U);
    //clahe->apply(BC_inv,BC_inv);

    cv::dft(GC, GC_dft, cv::DFT_REAL_OUTPUT);
    GC_ds = GC_dft.mul(wiener_filter);
    cv::dft(GC_ds, GC_inv, cv::DFT_INVERSE|cv::DFT_REAL_OUTPUT);
    cv::normalize(GC_inv, GC_inv, 0, 1, cv::NORM_MINMAX);
    //pow(BC_inv, 0.92, BC_inv);
    //normalize(GC_inv, GC_inv, 0, 255, CV_MINMAX);
    //GC_inv.convertTo(GC_inv, CV_8U);
    //clahe->apply(GC_inv,GC_inv);

    cv::dft(RC, RC_dft, cv::DFT_REAL_OUTPUT);
    RC_ds = RC_dft.mul(wiener_filter);
    cv::dft(RC_ds, RC_inv, cv::DFT_INVERSE|cv::DFT_REAL_OUTPUT);
    cv::normalize(RC_inv, RC_inv, 0, 1, CV_MINMAX);
//    pow(RC_inv, er, RC_inv);
    //normalize(RC_inv, RC_inv, 0, 255, NORM_MINMAX);
    //RC_ds.convertTo(RC_inv, CV_8U);
    //clahe->apply(RC_inv,RC_inv);

    cv::normalize(BC_inv, BC_inv, 0, 255, cv::NORM_MINMAX);
    BC_inv.convertTo(BC_inv, CV_8U);
    clahe->apply(BC_inv,BC_inv);
    normalize(GC_inv, GC_inv, 0, 255, cv::NORM_MINMAX);
    GC_inv.convertTo(GC_inv, CV_8U);
    clahe->apply(GC_inv,GC_inv);
    normalize(RC_inv, RC_inv, 0, 255, cv::NORM_MINMAX);
    RC_inv.convertTo(RC_inv, CV_8U);
    clahe->apply(RC_inv,RC_inv);

    BGR.push_back(BC_inv);
    BGR.push_back(GC_inv);
    BGR.push_back(RC_inv);

    cv::merge(BGR, img_ds);
    //normalize(img_ds, img_ds, 0, 255, NORM_MINMAX);
    //img_ds.convertTo(img_ds, CV_8U);

    return img_ds;
}

cv::Mat CFilt::clahe(cv::Mat img){

    vector<cv::Mat> BGR;
    cv::Mat BC, GC, RC, img_ds;
    cv::Ptr<cv::CLAHE> clahe = cv::createCLAHE();

    cv::split(img, BGR);
    BGR.back().copyTo(RC);
    BGR.pop_back();
    BGR.back().copyTo(GC);
    BGR.pop_back();
    BGR.back().copyTo(BC);
    BGR.pop_back();

    clahe->setClipLimit(3);
    clahe->apply(BC,BC);
    clahe->setClipLimit(3);
    clahe->apply(GC,GC);
    clahe->setClipLimit(3);
    clahe->apply(RC,RC);
    BGR.push_back(BC);
    BGR.push_back(GC);
    BGR.push_back(RC);
    cv::merge(BGR, img_ds);

    //imshow("img_ds", img_ds);
    return img_ds;
}
/********************************************/

class Detector {
 public:
  Detector(const string& model_file,
           const string& weights_file,
           const string& mean_file,
           const string& mean_value);

  std::vector<vector<float> > Detect(const cv::Mat& img);

 private:
  void SetMean(const string& mean_file, const string& mean_value);

  void WrapInputLayer(std::vector<cv::Mat>* input_channels);

  void Preprocess(const cv::Mat& img,
                  std::vector<cv::Mat>* input_channels);

 private:
  shared_ptr<Net<float> > net_;
  cv::Size input_geometry_;
  int num_channels_;
  cv::Mat mean_;
};

Detector::Detector(const string& model_file,
                   const string& weights_file,
                   const string& mean_file,
                   const string& mean_value) {
#ifdef CPU_ONLY
  Caffe::set_mode(Caffe::CPU);
#else
  Caffe::set_mode(Caffe::GPU);
#endif

  /* Load the network. */
  net_.reset(new Net<float>(model_file, TEST));
  net_->CopyTrainedLayersFrom(weights_file);

  CHECK_EQ(net_->num_inputs(), 1) << "Network should have exactly one input.";
  CHECK_EQ(net_->num_outputs(), 1) << "Network should have exactly one output.";

  Blob<float>* input_layer = net_->input_blobs()[0];
  num_channels_ = input_layer->channels();
  CHECK(num_channels_ == 3 || num_channels_ == 1)
    << "Input layer should have 1 or 3 channels.";
  input_geometry_ = cv::Size(input_layer->width(), input_layer->height());

  /* Load the binaryproto mean file. */
  SetMean(mean_file, mean_value);
}

std::vector<vector<float> > Detector::Detect(const cv::Mat& img) {
  Blob<float>* input_layer = net_->input_blobs()[0];
  input_layer->Reshape(1, num_channels_,
                       input_geometry_.height, input_geometry_.width);
  /* Forward dimension change to all layers. */
  net_->Reshape();

  std::vector<cv::Mat> input_channels;
  WrapInputLayer(&input_channels);

  Preprocess(img, &input_channels);

  net_->Forward();

  /* Copy the output layer to a std::vector */
  Blob<float>* result_blob = net_->output_blobs()[0];
  const float* result = result_blob->cpu_data();
  const int num_det = result_blob->height();
  vector<vector<float> > detections;
  for (int k = 0; k < num_det; ++k) {
    if (result[0] == -1) {
      // Skip invalid detection.
      result += 7;
      continue;
    }
    vector<float> detection(result, result + 7);
    detections.push_back(detection);
    result += 7;
  }
  return detections;
}

/* Load the mean file in binaryproto format. */
void Detector::SetMean(const string& mean_file, const string& mean_value) {
  cv::Scalar channel_mean;
  if (!mean_file.empty()) {
    CHECK(mean_value.empty()) <<
      "Cannot specify mean_file and mean_value at the same time";
    BlobProto blob_proto;
    ReadProtoFromBinaryFileOrDie(mean_file.c_str(), &blob_proto);

    /* Convert from BlobProto to Blob<float> */
    Blob<float> mean_blob;
    mean_blob.FromProto(blob_proto);
    CHECK_EQ(mean_blob.channels(), num_channels_)
      << "Number of channels of mean file doesn't match input layer.";

    /* The format of the mean file is planar 32-bit float BGR or grayscale. */
    std::vector<cv::Mat> channels;
    float* data = mean_blob.mutable_cpu_data();
    for (int i = 0; i < num_channels_; ++i) {
      /* Extract an individual channel. */
      cv::Mat channel(mean_blob.height(), mean_blob.width(), CV_32FC1, data);
      channels.push_back(channel);
      data += mean_blob.height() * mean_blob.width();
    }

    /* Merge the separate channels into a single image. */
    cv::Mat mean;
    cv::merge(channels, mean);

    /* Compute the global mean pixel value and create a mean image
     * filled with this value. */
    channel_mean = cv::mean(mean);
    mean_ = cv::Mat(input_geometry_, mean.type(), channel_mean);
  }
  if (!mean_value.empty()) {
    CHECK(mean_file.empty()) <<
      "Cannot specify mean_file and mean_value at the same time";
    stringstream ss(mean_value);
    vector<float> values;
    string item;
    while (getline(ss, item, ',')) {
      float value = std::atof(item.c_str());
      values.push_back(value);
    }
    CHECK(values.size() == 1 || values.size() == num_channels_) <<
      "Specify either 1 mean_value or as many as channels: " << num_channels_;

    std::vector<cv::Mat> channels;
    for (int i = 0; i < num_channels_; ++i) {
      /* Extract an individual channel. */
      cv::Mat channel(input_geometry_.height, input_geometry_.width, CV_32FC1,
          cv::Scalar(values[i]));
      channels.push_back(channel);
    }
    cv::merge(channels, mean_);
  }
}

/* Wrap the input layer of the network in separate cv::Mat objects
 * (one per channel). This way we save one memcpy operation and we
 * don't need to rely on cudaMemcpy2D. The last preprocessing
 * operation will write the separate channels directly to the input
 * layer. */
void Detector::WrapInputLayer(std::vector<cv::Mat>* input_channels) {
  Blob<float>* input_layer = net_->input_blobs()[0];

  int width = input_layer->width();
  int height = input_layer->height();
  float* input_data = input_layer->mutable_cpu_data();
  for (int i = 0; i < input_layer->channels(); ++i) {
    cv::Mat channel(height, width, CV_32FC1, input_data);
    input_channels->push_back(channel);
    input_data += width * height;
  }
}

void Detector::Preprocess(const cv::Mat& img,
                            std::vector<cv::Mat>* input_channels) {
  /* Convert the input image to the input image format of the network. */
  cv::Mat sample;
  if (img.channels() == 3 && num_channels_ == 1)
    cv::cvtColor(img, sample, cv::COLOR_BGR2GRAY);
  else if (img.channels() == 4 && num_channels_ == 1)
    cv::cvtColor(img, sample, cv::COLOR_BGRA2GRAY);
  else if (img.channels() == 4 && num_channels_ == 3)
    cv::cvtColor(img, sample, cv::COLOR_BGRA2BGR);
  else if (img.channels() == 1 && num_channels_ == 3)
    cv::cvtColor(img, sample, cv::COLOR_GRAY2BGR);
  else
    sample = img;

  cv::Mat sample_resized;
  if (sample.size() != input_geometry_)
    cv::resize(sample, sample_resized, input_geometry_);
  else
    sample_resized = sample;

  cv::Mat sample_float;
  if (num_channels_ == 3)
    sample_resized.convertTo(sample_float, CV_32FC3);
  else
    sample_resized.convertTo(sample_float, CV_32FC1);

  cv::Mat sample_normalized;
  cv::subtract(sample_float, mean_, sample_normalized);

  /* This operation will write the separate BGR planes directly to the
   * input layer of the network because it is wrapped by the cv::Mat
   * objects in input_channels. */
  cv::split(sample_normalized, *input_channels);

  CHECK(reinterpret_cast<float*>(input_channels->at(0).data)
        == net_->input_blobs()[0]->cpu_data())
    << "Input channels are not wrapping the input layer of the network.";
}

DEFINE_string(mean_file, "",
    "The mean file used to subtract from the input image.");
DEFINE_string(mean_value, "104,117,123",
    "If specified, can be one value or can be same as image channels"
    " - would subtract from the corresponding channel). Separated by ','."
    "Either mean_file or mean_value should be provided, not both.");
DEFINE_string(file_type, "image",
    "The file type in the list_file. Currently support image and video.");
DEFINE_string(out_file, "",
    "If provided, store the detection results in the out_file.");
DEFINE_double(confidence_threshold, 0.01,
    "Only store detections with score higher than the threshold.");
DEFINE_int32(RUAS, 1,
    "RUAS mode, 0: close; 1: filter+clahe; 2: clahe.");
DEFINE_double(intensity, 10,
    "turbulence intensity.");
DEFINE_double(SNR, 10,
    "Signal to Noise Ratio");

template <typename T>
string ToString(T val)
{
    stringstream stream;
    stream << val;
    return stream.str();
}

int main(int argc, char** argv) {

  ::google::InitGoogleLogging(argv[0]);
  // Print output to stderr (while still logging)
  FLAGS_alsologtostderr = 1;

#ifndef GFLAGS_GFLAGS_H_
  namespace gflags = google;
#endif

  gflags::SetUsageMessage("Do detection using SSD mode.\n"
        "Usage:\n"
        "    ssd_detect [FLAGS] model_file weights_file list_file\n");
  gflags::ParseCommandLineFlags(&argc, &argv, true);

  if (argc < 4) {
    gflags::ShowUsageWithFlagsRestrict(argv[0], "examples/ssd/ssd_detect");
    return 1;
  }

  const string& model_file = argv[1];
  const string& weights_file = argv[2];
  const string& mean_file = FLAGS_mean_file;
  const string& mean_value = FLAGS_mean_value;
  const string& file_type = FLAGS_file_type;
  const string& out_file = FLAGS_out_file;
  const float confidence_threshold = FLAGS_confidence_threshold;
  const int RUAS = FLAGS_RUAS;
  const float intensity = FLAGS_intensity;
  const float SNR = FLAGS_SNR;

  // Initialize the network.
  Detector detector(model_file, weights_file, mean_file, mean_value);

  int re_width = 640;
  int re_height = 480;
  CFilt filter(re_height, re_width);
  if(RUAS){
    filter.get_wf(intensity, SNR);
  }
  // Set the output mode.
  std::streambuf* buf = std::cout.rdbuf();
  std::ofstream outfile;
  if (!out_file.empty()) {
    outfile.open(out_file.c_str());
    if (outfile.good()) {
      buf = outfile.rdbuf();
    }
  }
  std::ostream out(buf);

  // Process image one by one.
  std::ifstream infile(argv[3]);
  std::string file;
  while (infile >> file) {
    if (file_type == "image") {
      std::cout<< file <<std::endl;
      cv::Mat img = cv::imread(file, -1);
      CHECK(!img.empty()) << "Unable to decode image " << file;
      std::vector<vector<float> > detections = detector.Detect(img);

      /* Print the detection results. */
      for (int i = 0; i < detections.size(); ++i) {
        const vector<float>& d = detections[i];
        // Detection format: [image_id, label, score, xmin, ymin, xmax, ymax].
        CHECK_EQ(d.size(), 7);
        const float score = d[2];
        if (score >= confidence_threshold) {
          out << file << " ";
          out << static_cast<int>(d[1]) << " ";
          out << score << " ";
          out << static_cast<int>(d[3] * img.cols) << " ";
          out << static_cast<int>(d[4] * img.rows) << " ";
          out << static_cast<int>(d[5] * img.cols) << " ";
          out << static_cast<int>(d[6] * img.rows) << std::endl;
        }
        if(score>confidence_threshold)
            cv::rectangle(img, cv::Point(d[3]*img.cols, d[4]*img.rows), cv::Point(d[5]*img.cols, d[6]*img.rows), cv::Scalar(200-d[1]*10, 100-d[1]*10, d[1]*10), 2);
            cv::putText(img, ToString(score), cv::Point(d[3]*img.cols, d[4]*img.rows), cv::FONT_HERSHEY_SIMPLEX, 1.0, cv::Scalar(d[1]*50, 100-d[1]*20, 200-d[1]*50), 2);
      }
      cv::imshow(file, img);
      cv::waitKey(0);

    } else if (file_type == "video") {
      int re_width = 640;
      int re_height = 480;
      cv::VideoCapture cap(file);
      /********** record setting ***************/
      char EXT[] = "MJPG";
      int ex1 = EXT[0] | (EXT[1] << 8) | (EXT[2] << 16) | (EXT[3] << 24);
      cv::VideoWriter record;
//      record.open(out_file, ex1, 25, cv::Size(cap.get(CV_CAP_PROP_FRAME_WIDTH), cap.get(CV_CAP_PROP_FRAME_HEIGHT)), true);
      record.open(out_file, ex1, 25, cv::Size(re_width, re_height), true);
      if(!record.isOpened()){
             std::cout << "Can not open the output video for write" << std::endl;
      }
      if (!cap.isOpened()) {
        LOG(FATAL) << "Failed to open video: " << file;
      }

      cv::Mat img;
      int frame_count = 0;
      while (true) {
        bool success = cap.read(img);
        if (!success) {
          LOG(INFO) << "Process " << frame_count << " frames from " << file;
          break;
        }
        CHECK(!img.empty()) << "Error when read frame";
        clock_t s, t;
        s = clock();
        srand((int)time(0));
        cv::resize(img, img, cv::Size(re_width, re_height));
        if(RUAS == 1)
            img = filter.wiener(img);
        else if(RUAS == 2)
            img = filter.clahe(img);
        std::vector<vector<float> > detections = detector.Detect(img);
        t = clock();
        double fps = 1/((double)(t-s)/CLOCKS_PER_SEC);

        /* Print the detection results. */
        for (int i = 0; i < detections.size(); ++i) {
          const vector<float>& d = detections[i];
          // Detection format: [image_id, label, score, xmin, ymin, xmax, ymax].
          CHECK_EQ(d.size(), 7);
          const float score = d[2];
          if (score >= confidence_threshold) {
            out << file << "_";
            out << std::setfill('0') << std::setw(6) << frame_count << " ";
            out << static_cast<int>(d[1]) << " ";
            out << score << " ";
            out << static_cast<int>(d[3] * img.cols) << " ";
            out << static_cast<int>(d[4] * img.rows) << " ";
            out << static_cast<int>(d[5] * img.cols) << " ";
            out << static_cast<int>(d[6] * img.rows) << std::endl;
          }
          if(score>confidence_threshold){
            cv::rectangle(img, cv::Point(d[3]*img.cols, d[4]*img.rows), cv::Point(d[5]*img.cols, d[6]*img.rows), cv::Scalar(200-d[1]*50, 100-d[1]*20, d[1]*50), 2);
            cv::putText(img, ToString(score), cv::Point(d[3]*img.cols, d[4]*img.rows), cv::FONT_HERSHEY_SIMPLEX, 0.8, cv::Scalar(200-d[1]*50, 100-d[1]*20, d[1]*50), 2);
          }
        }
        cv::putText(img, ToString(fps), cv::Point(20, 20), cv::FONT_HERSHEY_SIMPLEX, 0.8, cv::Scalar(255, 255, 255), 2);
        cv::imshow(file, img);
        record << img;
        cv::waitKey(1);
        ++frame_count;
      }
      if (cap.isOpened()) {
        cap.release();
      }
      if(record.isOpened()) {
        record.release();
      }
    } else {
      LOG(FATAL) << "Unknown file_type: " << file_type;
    }
  }
  return 0;
}
